import serial
import numpy as np
import cv2

def rgb565_to_bgr888(rgb565):
    """将 RGB565 转换为 BGR888"""
    r = (rgb565 >> 11) & 0x1F
    g = (rgb565 >> 5) & 0x3F
    b = rgb565 & 0x1F

    r = (r * 255) // 31
    g = (g * 255) // 63
    b = (b * 255) // 31

    return [b, g, r]  # OpenCV 使用 BGR 顺序

def parse_frame(ser):
    while True:
        line = ser.readline().decode(errors='ignore').strip()
        if line.startswith("FRAME_START"):
            frame_id = int(line.split(":")[1])
            print(f"[INFO] 接收帧: {frame_id}")
            break

    while ser.readline().decode(errors='ignore').strip() != "DATA_START":
        pass

    img_data = bytearray()
    pixel_count = 320 * 240

    # 读取 76800 * 2 = 153600 字节
    while len(img_data) < pixel_count * 2:
        chunk = ser.read(pixel_count * 2 - len(img_data))
        if not chunk:
            print("[ERROR] 读取数据失败")
            return None
        img_data.extend(chunk)

    while ser.readline().decode(errors='ignore').strip() != "DATA_END":
        pass

    ser.readline()  # FRAME_END

    # 将数据转换为图像
    img = np.zeros((240, 320, 3), dtype=np.uint8)
    for i in range(pixel_count):
        byte1 = img_data[i * 2]
        byte2 = img_data[i * 2 + 1]
        rgb565 = (byte1 << 8) | byte2
        img[i // 320, i % 320] = rgb565_to_bgr888(rgb565)

    return img

def main():
    ser = serial.Serial('/dev/ttyUSB0', 921600, timeout=2)
    print("[INFO] 串口已打开")

    while True:
        img = parse_frame(ser)
        if img is not None:
            cv2.imshow('OV7670 Frame', img)
            key = cv2.waitKey(1)
            if key == ord('s'):
                cv2.imwrite('saved_frame.jpg', img)
            elif key == ord('q'):
                break

    ser.close()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()
